Restore G61/G61.1/G64 state after abort

After abort, restore motion control mode and blend/naive CAM
tolerances to state of last executing motion before the abort.  This
depends on Rob Ellenberg's state tags work.

The `state_tag_t` struct keeps integer settings in an array, indexed
by the `StateField` enum.  Introducing the `G64 P* Q*` tolerances adds
two more `float` types to the existing two for feed and speed.  This
patch packs those all into a new array of floats, indexed by a new
enum, to simplify handling.  These new floats are exposed in the
python module `status.settings` attribute, positions 3 and 4.

Restoring this state has two parts.  The motion control mode
G61/G61.1/G64 would have been handled by `Interp::gen_g_codes()`, and
the G64 blend/naive CAM tolerances would have been handled by
`Interp::gen_settings()`.  However, a switch from G61/G61.1 to G64 and
a simultaneous switch of P/Q tolerance values cannot be done with two
independent functions.  Therefore, `gen_g_codes()` has been merged
into `gen_settings()` in order to handle this special case.

To keep the tolerances updated during program execution, they are
added to the `Interp::synch()` function.

To restore the naive CAM tolerance, a new function
`GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE()` is introduced,
parallel to `GET_EXTERNAL_MOTION_CONTROL_TOLERANCE()` for blend
tolerance.

This patch fixes #177, "Forgetting G64 mode after E-stop," and one of
the remaining issues in #134, "Updating `state-tags-master` branch for
acceptance into LinuxCNC."

Signed-off-by: John Morris <john@zultron.com>

docs/src/config/python-interface.txt

@@ -336,7 +336,9 @@ current XY rotation angle around Z axis.
 
 *settings*:: '(returns tuple of floats)' -
 current interpreter settings. settings[0] =
-sequence number, settings[1] = feed rate, settings[2] = speed.
+sequence number, settings[1] = feed rate, settings[2] = speed,
+settings[3] = `G64 P` blend tolerance,
+settings[4] = `G64 Q` naive CAM tolerance.
 
 *spindle*:: ' (returns tuple of dicts) ' -
 returns the current spindle status

src/emc/motion/state_tag.h

@@ -62,7 +62,7 @@ typedef enum {
 
 
 /**
- * Enum for various fields of state info that aren't binary.
+ * Enum for various fields of state info that are int type.
  *
  * WARNING:
  *
@@ -87,6 +87,20 @@ typedef enum {
     GM_FIELD_MAX_FIELDS
 } StateField;
 
+
+/**
+ * Enum for indexing state tag `fields_float`, machine state float
+ * array:  feed, speed, etc.
+ */
+typedef enum {
+    GM_FIELD_FLOAT_LINE_NUMBER, // eww
+    GM_FIELD_FLOAT_FEED,
+    GM_FIELD_FLOAT_SPEED,
+    GM_FIELD_FLOAT_PATH_TOLERANCE,
+    GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE,
+    GM_FIELD_FLOAT_MAX_FIELDS
+} StateFieldFloat;
+
 /**
  * Tag structure that is added to a motion segment so that motion has a copy of
  * the relevant interp state.
@@ -99,9 +113,9 @@ typedef enum {
  */
 struct state_tag_t {
 
-    // Machine settings 
-    float feed;
-    float speed;
+    // Float-type machine settings:  feed, speed, etc., indexed by the
+    // StateFieldFloat enum above
+    float fields_float[GM_FIELD_FLOAT_MAX_FIELDS];
 
     // Any G / M code states that doesn't pack nicely into a single bit
     // These are an array mostly because it's easier to pass an

src/emc/nml_intf/canon.hh

@@ -794,6 +794,9 @@ extern CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE();
 // Returns the current motion path-following tolerance
 extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE();
 
+// Returns the current motion naive CAM tolerance
+extern double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE();
+
 /* The interpreter is not using these six GET_EXTERNAL_ORIGIN functions
 
 // returns the current a-axis origin offset

src/emc/rs274ngc/canonmodule.cc

@@ -136,6 +136,8 @@ BOOST_PYTHON_MODULE(emccanon) {
     def("GET_EXTERNAL_MOTION_CONTROL_MODE",&GET_EXTERNAL_MOTION_CONTROL_MODE);
     def("GET_EXTERNAL_MOTION_CONTROL_TOLERANCE",&GET_EXTERNAL_MOTION_CONTROL_TOLERANCE);
     def("SET_PARAMETER_FILE_NAME",&SET_PARAMETER_FILE_NAME);
+    def("GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE",
+	&GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE);
     def("GET_EXTERNAL_PARAMETER_FILE_NAME",&GET_EXTERNAL_PARAMETER_FILE_NAME);
     def("GET_EXTERNAL_PLANE",&GET_EXTERNAL_PLANE);
     def("GET_EXTERNAL_POCKETS_MAX",&GET_EXTERNAL_POCKETS_MAX);

src/emc/rs274ngc/gcodemodule.cc

@@ -498,6 +498,7 @@ void RIGID_TAP(int line_number,
     Py_XDECREF(result);
 }
 double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return 0.1; }
+double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE() { return 0.1; }
 double GET_EXTERNAL_PROBE_POSITION_X() { return _pos_x; }
 double GET_EXTERNAL_PROBE_POSITION_Y() { return _pos_y; }
 double GET_EXTERNAL_PROBE_POSITION_Z() { return _pos_z; }

src/emc/rs274ngc/interp_base.hh

@@ -27,7 +27,7 @@
 /* Size of certain arrays */
 #define ACTIVE_G_CODES 17
 #define ACTIVE_M_CODES 10
-#define ACTIVE_SETTINGS 3
+#define ACTIVE_SETTINGS 5
 
 class InterpBase : boost::noncopyable {
 public:

src/emc/rs274ngc/interp_convert.cc

@@ -1561,11 +1561,13 @@ already in force.
 
 */
 
-int Interp::convert_control_mode(int g_code,     //!< g_code being executed (G_61, G61_1, || G_64)
-				double tolerance,    //tolerance for the path following in G64
-				double naivecam_tolerance,    //tolerance for the naivecam
-                                setup_pointer settings) //!< pointer to machine settings                 
+int Interp::convert_control_mode(
+    int g_code,                   // g_code being executed (G_61, G61_1, G_64)
+    double tolerance_in,          // tolerance for the path following in G64
+    double naivecam_tolerance_in, // tolerance for the naivecam
+    setup_pointer settings)       // pointer to machine settings                 
 {
+    double tolerance, naivecam_tolerance;
   CHKS((settings->cutter_comp_side),
        (_("Cannot change control mode with cutter radius compensation on")));
   if (g_code == G_61) {
@@ -1575,19 +1577,24 @@ int Interp::convert_control_mode(int g_code,     //!< g_code being executed (G_6
     SET_MOTION_CONTROL_MODE(CANON_EXACT_STOP, 0);
     settings->control_mode = CANON_EXACT_STOP;
   } else if (g_code == G_64) {
-	if (tolerance >= 0) {
-	    SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, tolerance);
-	} else {
-	    SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0);
-	}
-	if (naivecam_tolerance >= 0) {
-	    SET_NAIVECAM_TOLERANCE(naivecam_tolerance);
-	} else if (tolerance >= 0) {
-	    SET_NAIVECAM_TOLERANCE(tolerance);   // if no naivecam_tolerance specified use same for both
-	} else {
-	    SET_NAIVECAM_TOLERANCE(0);
-	}
-    settings->control_mode = CANON_CONTINUOUS;
+      if (tolerance_in >= 0)
+	  tolerance = tolerance_in;
+      else
+	  tolerance = 0;
+      settings->control_mode = CANON_CONTINUOUS;
+      settings->tolerance = tolerance;
+      SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, tolerance);
+
+      if (naivecam_tolerance_in >= 0)
+	  naivecam_tolerance = naivecam_tolerance_in;
+      else if (tolerance_in >= 0)
+	  // if no naivecam_tolerance specified use same for both
+	  naivecam_tolerance = tolerance_in;
+      else
+	  naivecam_tolerance = 0;
+      settings->naivecam_tolerance = naivecam_tolerance;
+      SET_NAIVECAM_TOLERANCE(naivecam_tolerance);
+
   } else 
     ERS(NCE_BUG_CODE_NOT_G61_G61_1_OR_G64);
   return INTERP_OK;
@@ -2708,43 +2715,48 @@ int Interp::convert_length_units(int g_code,     //!< g_code being executed (mus
 
 
 /*
- * given two double arrays representing interpreter settings as stored in
- * _setup.active_settings, construct a G-code sequence to synchronize their state.
+ * given two int arrays and two double arrays representing interpreter
+ * settings as stored in _setup.active_g_codes and
+ * _setup.active_settings, construct a G-code sequence to synchronize
+ * their state.
  */
-int Interp::gen_settings(double *current, double *saved, std::string &cmd)
+int Interp::gen_settings(
+    int *int_current, int *int_saved,            // G codes
+    double *float_current, double *float_saved,  // S, F, other
+    std::string &cmd)                            // command buffer
 {
-    int i;
+    int i, val;
+    int g64_changed = 0;
     char buf[LINELEN];
+
+    // F, S
     for (i = 0; i < ACTIVE_SETTINGS; i++) {
-	if (saved[i] != current[i]) {
+	if (float_saved[i] != float_current[i]) {
 	    switch (i) {
-	    case 0: break; // sequence_number - no point in restoring
-	    case 1:
-		snprintf(buf,sizeof(buf)," F%.1f", saved[i]);
+	    case GM_FIELD_FLOAT_LINE_NUMBER:
+		// sequence_number - no point in restoring
+		break;
+	    case GM_FIELD_FLOAT_FEED:
+		snprintf(buf,sizeof(buf)," F%.1f", float_saved[i]);
                 cmd += buf;
 		break;
-	    case 2:
-		snprintf(buf,sizeof(buf)," S%.0f", saved[i]);
+	    case GM_FIELD_FLOAT_SPEED:
+		snprintf(buf,sizeof(buf)," S%.0f", float_saved[i]);
                 cmd += buf;
 		break;
+	    case GM_FIELD_FLOAT_PATH_TOLERANCE:
+	    case GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE:
+		// G64 special case; see below
+		g64_changed = 1;
+		break;
 	    }
 	}
     }
-    return INTERP_OK;
-}
-
 
-/*
- * given two int arrays representing interpreter settings as stored in
- * _setup.active_g_codes, construct a G-code sequence to synchronize their state.
- */
-int Interp::gen_g_codes(int *current, int *saved, std::string &cmd)
-{
-    int i, val;
-    char buf[LINELEN];
+    // G codes
     for (i = 0; i < ACTIVE_G_CODES; i++) {
-	val = saved[i];
-	if (val != current[i]) {
+	val = int_saved[i];
+	if (val != int_current[i]) {
 
 	    switch (i) {
 	    case 0:
@@ -2768,7 +2780,6 @@ int Interp::gen_g_codes(int *current, int *saved, std::string &cmd)
 	    case 8: // - coordinate system
 	    case 9: // - tool offset (G43/G49)
 	    case 10: // - retract mode
-	    case 11: // - control mode
 	    case 13: // - spindle mode
 	    case 14: // - ijk distance mode
 	    case 15: // - lathe diameter mode
@@ -2785,12 +2796,50 @@ int Interp::gen_g_codes(int *current, int *saved, std::string &cmd)
                     cmd += buf;
 		} else {
 		    // so complain rather loudly
-		    MSG("------ gen_g_codes BUG: index %d = -1!!\n",i);
+		    MSG("------ gen_settings BUG: index %d = -1!!\n",i);
 		}
 		break;
+	    case 11: // - control mode
+		// Special case, since G64 requires P and Q flags
+		// FIXME what about when P or Q changes, even though still G64?
+		if (val == G_64)
+		    // G64 special case; see below
+		    g64_changed = 1;
+		else if (val == G_61) {
+		    snprintf(buf,sizeof(buf)," G61");
+		    cmd += buf;
+		} else if (val == G_61_1) {
+		    snprintf(buf,sizeof(buf)," G61.1");
+		    cmd += buf;
+		} else
+		    MSG("------ gen_settings BUG: index %d = -1!!\n",i);
+		break;
 	    }
 	}
     }
+
+
+    // Special case for restoring G64:  `cmd` may contain a `G64` if
+    // current int settings is `G61` or if current float settings
+    // contain different `G64 P* Q*` args; in these cases, only add a
+    // single `G64` command
+    if ((int_saved[11] == G_64) && g64_changed) {
+	if (float_saved[GM_FIELD_FLOAT_PATH_TOLERANCE] < 0)
+	    // No P, Q args
+	    snprintf(buf,sizeof(buf)," G64");
+	else if (
+	    float_saved[GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE] < 0)
+	    // Only P arg
+	    snprintf(buf,sizeof(buf)," G64 P%f",
+		     float_saved[GM_FIELD_FLOAT_PATH_TOLERANCE]);
+	else  // Both P, Q args
+	    snprintf(
+		buf,sizeof(buf)," G64 P%f Q%f",
+		float_saved[GM_FIELD_FLOAT_PATH_TOLERANCE],
+		float_saved[GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE]);
+	cmd += buf;
+    }
+
     return INTERP_OK;
 }
 
@@ -2852,12 +2901,14 @@ int Interp::gen_restore_cmd(int *current_g,
 			    StateTag const &saved,
 			    std::string &cmd)
 {
+    int res;
     // A local copy of the saved settings, unpacked from a state tag
     int saved_g[ACTIVE_G_CODES];
     int saved_m[ACTIVE_M_CODES];
     double saved_settings[ACTIVE_SETTINGS];
 
     //Extract saved state to local vectors
+    // FIXME Use saved_settings to store state tag floats, incl. in 
     int res_unpack = active_modes(saved_g, saved_m, saved_settings, saved);
     if (res_unpack != INTERP_OK) {
 	logStateTags("gen_restore_cmd() error %d:  failed to unpack state tag",
@@ -2880,17 +2931,14 @@ int Interp::gen_restore_cmd(int *current_g,
 	     cmd.c_str());
     }
 
-    int res_settings = gen_settings(current_settings, saved_settings, cmd);
-    //M codes should not be restored during an abort
-    int res_m = 0;
-    int res_g = gen_g_codes(current_g, saved_g, cmd);
-
-    if (res_settings || res_m || res_g) {
-	logStateTags("gen_restore_cmd():  error restoring "
-		     "settings (%d) or G codes (%d)",
-		     res_settings, res_g);
+    if ((res = gen_settings(
+	     current_g, saved_g, current_settings, saved_settings, cmd))) {
+	logStateTags("gen_restore_cmd():  error restoring settings (%d)",
+		     res);
         return INTERP_ERROR;
     }
+    // M codes should not be restored during an abort with gen_m_codes()
+
     return INTERP_OK;
 }
 
@@ -2952,9 +3000,16 @@ int Interp::restore_settings(setup_pointer settings,
 	snprintf(buf,sizeof(buf), "G%d",settings->sub_context[from_level].saved_g_codes[5]/10);
 	CHKS(execute(buf) != INTERP_OK, _("M7x: restore_settings G20/G21 failed: '%s'"), cmd.c_str());
     }
-    gen_settings((double *)settings->active_settings, (double *)settings->sub_context[from_level].saved_settings,cmd);
-    gen_m_codes((int *) settings->active_m_codes, (int *)settings->sub_context[from_level].saved_m_codes,cmd);
-    gen_g_codes((int *)settings->active_g_codes, (int *)settings->sub_context[from_level].saved_g_codes,cmd);
+    gen_settings(
+	(int *)settings->active_g_codes,
+	(int *)settings->sub_context[from_level].saved_g_codes,
+	(double *)settings->active_settings,
+	(double *)settings->sub_context[from_level].saved_settings,
+	cmd);
+    gen_m_codes(
+	(int *) settings->active_m_codes,
+	(int *)settings->sub_context[from_level].saved_m_codes,
+	cmd);
 
     if (!cmd.empty()) {
 	// the sequence can be multiline, separated by nl

src/emc/rs274ngc/interp_internal.hh

@@ -681,6 +681,8 @@ struct setup
 
   char blocktext[LINELEN];   // linetext downcased, white space gone
   CANON_MOTION_MODE control_mode;       // exact path or cutting mode
+    double tolerance;           // G64 blending tolerance
+    double naivecam_tolerance;  // G64 naive cam tolerance
   int current_pocket;             // carousel slot number of current tool
   double current_x;             // current X-axis position
   double current_y;             // current Y-axis position

src/emc/rs274ngc/interp_write.cc

@@ -114,6 +114,7 @@ int Interp::write_g_codes(block_pointer block,   //!< pointer to a block of RS27
             settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c ||
             settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? G_43 : G_49;
   gez[10] = (settings->retract_mode == OLD_Z) ? G_98 : G_99;
+  // Three modes:  G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP
   gez[11] =
     (settings->control_mode == CANON_CONTINUOUS) ? G_64 :
     (settings->control_mode == CANON_EXACT_PATH) ? G_61 : G_61_1;
@@ -197,9 +198,11 @@ int Interp::write_settings(setup_pointer settings)       //!< pointer to machine
   double *vals;
 
   vals = settings->active_settings;
-  vals[0] = settings->sequence_number;  /* 0 sequence number */
-  vals[1] = settings->feed_rate;        /* 1 feed rate       */
-  vals[2] = settings->speed[0];    /* 2 spindle speed   */
+  vals[0] = settings->sequence_number;    /* 0 sequence number */
+  vals[1] = settings->feed_rate;          /* 1 feed rate       */
+  vals[2] = settings->speed;              /* 2 spindle speed   */
+  vals[3] = settings->tolerance;          /* 3 blend tolerance */
+  vals[4] = settings->naivecam_tolerance; /* 4 naive CAM tolerance */
 
   return INTERP_OK;
 }
@@ -279,10 +282,16 @@ int Interp::write_state_tag(block_pointer block,
 	 settings->tool_offset.w);
     state.flags[GM_FLAG_RETRACT_OLDZ] =
 	(settings->retract_mode == OLD_Z);
+
     state.flags[GM_FLAG_BLEND] =
 	(settings->control_mode == CANON_CONTINUOUS);
     state.flags[GM_FLAG_EXACT_STOP] =
 	(settings->control_mode == CANON_EXACT_STOP);
+    state.fields_float[GM_FIELD_FLOAT_PATH_TOLERANCE] =
+	settings->tolerance;
+    state.fields_float[GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE] =
+	settings->naivecam_tolerance;
+
     state.flags[GM_FLAG_CSS_MODE] =
 	(settings->spindle_mode == CONSTANT_RPM);
     state.flags[GM_FLAG_IJK_ABS] =
@@ -312,8 +321,8 @@ int Interp::write_state_tag(block_pointer block,
 
     state.flags[GM_FLAG_FEED_HOLD] =  (settings->feed_hold);
 
-    state.feed = settings->feed_rate;        /* 1 feed rate       */
-    state.speed = settings->speed;    /* 2 spindle speed   */
+    state.fields_float[GM_FIELD_FLOAT_FEED] = settings->feed_rate;
+    state.fields_float[GM_FIELD_FLOAT_SPEED] = settings->speed;
 
     return 0;
 }